/*
 * minimum_snap.hpp
 *
 *  Created on: 2022年1月27日
 *      Author: huanyu Zhang
 */

#ifndef MODULES_TRAJECTORY_MINIMUM_SNAP_HPP_
#define MODULES_TRAJECTORY_MINIMUM_SNAP_HPP_

#include <Eigen>
#include <iostream>
#include <ctime>
#include "system/system.hpp"
#include "userlib/userlib.hpp"
using namespace std;
using namespace Eigen;

typedef typename Eigen::MatrixXd MatXd;
typedef typename Eigen::VectorXd VecXd;
typedef typename Eigen::Vector2d Vec2d;

// refer to: 《Polynomial Trajectory Planning for Aggressive
//             Quadrotor Flight in Dense Indoor Environments》

class MinimumSnap {
public:
    MinimumSnap(unsigned int order, double max_vel, double max_accel);

    MinimumSnap() = delete;

    ~MinimumSnap() = default;

    VecXd AllocateTime(const MatXd& waypoint) const;

    MatXd SolveQPClosedForm(const VecXd& waypoints, const VecXd& vel, const VecXd& acc, const VecXd& time) const;

    MatrixXd polydiff(MatrixXd& poly_coeff_mat);

    void getCommandPosVelAccYaw(double* res, const MatrixXd& poly_coeff_mat_x, const MatrixXd& poly_coeff_mat_y, const MatrixXd& poly_coeff_diff_mat_x, const MatrixXd& poly_coeff_diff_mat_y, const MatrixXd& poly_coeff_acc_mat_x, const MatrixXd& poly_coeff_acc_mat_y,int k, double t);

    unsigned int GetPolyCoeffNum() const;

private:
    const unsigned int order_;// =2: accel | =3: jerk | =4: snap
    const double max_vel_;
    const double max_accel_;
    const unsigned int num_poly_coeff;
};

extern MinimumSnap minimumSnapObj;

#endif /* MODULES_TRAJECTORY_MINIMUM_SNAP_HPP_ */
